/********************************** (C) COPYRIGHT *******************************
* File Name          : pid.c
* Author             : nx666
* Version            : V1.0.0
* Date               : 2023/08/5
* Description        : pid相关接口在这里实现
*******************************************************************************/
/***********************************头文件区域***********************************/
#include "pid.h"
#include "string.h"
/***********************************类型定义*********************************/


/***********************************宏定义区***********************************/


/******************************** *全局变量区**********************************/

 /*********************************************************************
 * @fn      pidCalc
 *
 * @brief   pid计算环节，
 *              void
 *
 * @return  pid计算结果
 *          
 */
static _iq pidCalc(struct pid *pPid, _iq error)
{
    _iq uk = 0;
    pPid->Integral += error;
    if (pPid->kp)
    {
        uk += _IQ12mpy(pPid->kp, error);
    }
    if (pPid->ki)
    {
        uk += _IQ12mpy(pPid->ki, pPid->Integral);
    }
    if (pPid->kd)
    {
        uk += _IQ12mpy(pPid->kd, error - pPid->lastError);
        pPid->lastError = error;
    }
    if (PID_TYPE_INC == pPid->type)
    {
        pPid->uk += uk;
        uk = pPid->uk;
    }
    return uk;
}

 /*********************************************************************
 * @fn      setType
 *
 * @brief   设置pid类型，
 *              void
 *
 * @return  pid计算结果
 *          
 */
static void setType(struct pid *pPid, pdi_type_ type)
{
    pPid->type = type;
}

 /*********************************************************************
 * @fn      pid_setKpKiKd
 *
 * @brief   设置pid参数，
 *              void
 *
 * @return  smo_ *，成功  NULL，失败
 *          
 */
static Int pid_setKpKiKd(struct pid *pPid, _iq kp, _iq ki, _iq kd)
{
    if (pPid->kp != kp)
    {
        pPid->kp = kp;
    }

    if (pPid->ki != ki)
    {
        pPid->ki = ki;
    }

    if (pPid->kd != kd)
    {
        pPid->kd = kd;
    }


}


 /*********************************************************************
 * @fn      pidCreate
 *
 * @brief   pid构造函数，
 *              void
 *
 * @return  smo_ *，成功  NULL，失败
 *          
 */
pid_ *pidCreate(_iq kp, _iq ki, _iq kd)
{
    pid_ *pPid = NULL;

    pPid = (pid_ *)malloc(sizeof(pid_));
    if (!pPid)
    {
        return NULL;
    }
    memset((void *)pPid, 0, sizeof(pid_));

    pPid->kp = kp;
    pPid->ki = ki;
    pPid->kd = kd;
    pPid->type = PID_TYPE_POS;
    pPid->calc = pidCalc;
    pPid->setType = setType;

    return pPid;
}